// Devin Koepl, Kevin Kemper

#include <fca_controllers/control_switcher_state_machine.h>

extern void control_switcher_state_machine(ControllerInput *controller_input, ControllerOutput *controller_output, 
	ControllerState *controller_state, ControllerData *controller_data)
{
	switch (controller_state->state)
	{
		case CSSM_STATE_DISABLED:
			if (controller_data->command == CMD_RUN)
			{
				controller_state->state = CSSM_STATE_ENABLED;
				PRINT_MSG( "Controller enabled.\n" );
			}
			else if ((controller_data->command == CMD_DISABLE) 
			&& (controller_data->controller_requested != controller_state->controller_loaded))
			{
				// The user requests a different controller.
				takedown_controller(controller_input, controller_output, controller_state, controller_data);
				switch_controllers(controller_state, controller_data);
				initialize_controller(controller_input, controller_output, controller_state, controller_data);
				PRINT_MSG( "Switching to controller %u.\n", controller_data->controller_requested );
			}

			break;
		case CSSM_STATE_ERROR:
			if (controller_data->command == CMD_DISABLE)
			{
				controller_state->state = CSSM_STATE_DISABLED;
				PRINT_MSG("Error -> Disabled.\n");
			}

			break;
		case CSSM_STATE_ENABLED:
			if (controller_data->command == CMD_DISABLE)
			{
				controller_state->state = CSSM_STATE_DISABLED;
				PRINT_MSG("Controller disabled.\n");
			}

			update_controller(controller_input, controller_output, controller_state, controller_data);

			// Return to the wrapper now.
			return;
		case CSSM_STATE_INIT:
			initialize_controller = &initialize_no_controller;
			update_controller = &update_no_controller;
			takedown_controller = &takedown_no_controller;

			initialize_controller(controller_input, controller_output, controller_state, controller_data);

			controller_state->state = CSSM_STATE_DISABLED;

			PRINT_MSG("Initializing controller.\n");

		break;
		case CSSM_STATE_FINI:
			takedown_controller(controller_input, controller_output, controller_state, controller_data);

			controller_state->state = CSSM_STATE_DISABLED;

			PRINT_MSG("Taking down controller.\n");

		break;
	}

	controller_output->motor_torque = 0.0;
}

extern void switch_controllers(ControllerState * controller_state, ControllerData * controller_data)
{
  switch (controller_data->controller_requested)
	{
		case NO_CONTROLLER:
		  initialize_controller = &initialize_no_controller;
		  update_controller = &update_no_controller;
		  takedown_controller = &takedown_no_controller;
		  break;
		case MOTOR_TORQUE_CONTROLLER:
		  initialize_controller = &initialize_motor_torque_controller;
		  update_controller = &update_motor_torque_controller;
		  takedown_controller = &takedown_motor_torque_controller;
		  break;
		case MOTOR_POSITION_CONTROLLER:
		  initialize_controller = &initialize_motor_position_controller;
		  update_controller = &update_motor_position_controller;
		  takedown_controller = &takedown_motor_position_controller;
		  break;
		case FORCE_CONTROLLER:
		  initialize_controller = &initialize_force_controller;
		  update_controller = &update_force_controller;
		  takedown_controller = &takedown_force_controller;
		  break;
		case TORQUE_SINE_CONTROLLER:
		  initialize_controller = &initialize_torque_sine_controller;
		  update_controller = &update_torque_sine_controller;
		  takedown_controller = &takedown_torque_sine_controller;
		  break;
		case POSITION_SINE_CONTROLLER:
		  initialize_controller = &initialize_position_sine_controller;
		  update_controller = &update_position_sine_controller;
		  takedown_controller = &takedown_position_sine_controller;
		  break;
		case OPENLOOP_SINE_CONTROLLER:
		  initialize_controller = &initialize_openloop_sine_controller;
		  update_controller = &update_openloop_sine_controller;
		  takedown_controller = &takedown_openloop_sine_controller;
		  break;
	}

	controller_state->controller_loaded = controller_data->controller_requested;
}
